//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty.  In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
//    claim that you wrote the original software. If you use this software
//    in a product, an acknowledgment in the product documentation would be
//    appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
//    misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//

#include <math.h>
#include <float.h>
#include <string.h>
#include "DetourNavMeshQuery.h"
#include "DetourNavMesh.h"
#include "DetourNode.h"
#include "DetourCommon.h"
#include "DetourAlloc.h"
#include "DetourAssert.h"
#include <new>


dtQueryFilter::dtQueryFilter() :
    m_includeFlags(0xffff),
    m_excludeFlags(0)
{
    for (int i = 0; i < DT_MAX_AREAS; ++i)
        m_areaCost[i] = 1.0f;
}

#ifdef DT_VIRTUAL_QUERYFILTER
bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/,
                               const dtMeshTile* /*tile*/,
                               const dtPoly* poly) const
{
    return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0;
}

float dtQueryFilter::getCost(const float* pa, const float* pb,
                             const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/,
                             const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly,
                             const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const
{
    return dtVdist(pa, pb) * m_areaCost[curPoly->area];
}
#else
inline bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/,
                                      const dtMeshTile* /*tile*/,
                                      const dtPoly* poly) const
{
    return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0;
}

inline float dtQueryFilter::getCost(const float* pa, const float* pb,
                                    const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/,
                                    const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly,
                                    const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const
{
    return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()];
}
#endif

static const float H_SCALE = 2.0f; // Search heuristic scale.


dtNavMeshQuery* dtAllocNavMeshQuery()
{
    void* mem = dtAlloc(sizeof(dtNavMeshQuery), DT_ALLOC_PERM);
    if (!mem) return 0;
    return new(mem) dtNavMeshQuery;
}

void dtFreeNavMeshQuery(dtNavMeshQuery* navmesh)
{
    if (!navmesh) return;
    navmesh->~dtNavMeshQuery();
    dtFree(navmesh);
}

//////////////////////////////////////////////////////////////////////////////////////////
dtNavMeshQuery::dtNavMeshQuery() :
    m_tinyNodePool(0),
    m_nodePool(0),
    m_openList(0)
{
    memset(&m_query, 0, sizeof(dtQueryData));
}

dtNavMeshQuery::~dtNavMeshQuery()
{
    if (m_tinyNodePool)
        m_tinyNodePool->~dtNodePool();
    if (m_nodePool)
        m_nodePool->~dtNodePool();
    if (m_openList)
        m_openList->~dtNodeQueue();
    dtFree(m_tinyNodePool);
    dtFree(m_nodePool);
    dtFree(m_openList);
}

dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes)
{
    m_nav = nav;

    if (!m_nodePool || m_nodePool->getMaxNodes() < maxNodes)
    {
        if (m_nodePool)
        {
            m_nodePool->~dtNodePool();
            dtFree(m_nodePool);
            m_nodePool = 0;
        }
        m_nodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(maxNodes, dtNextPow2(maxNodes/4));
        if (!m_nodePool)
            return DT_FAILURE | DT_OUT_OF_MEMORY;
    }
    else
    {
        m_nodePool->clear();
    }

    if (!m_tinyNodePool)
    {
        m_tinyNodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(64, 32);
        if (!m_tinyNodePool)
            return DT_FAILURE | DT_OUT_OF_MEMORY;
    }
    else
    {
        m_tinyNodePool->clear();
    }

    // TODO: check the open list size too.
    if (!m_openList || m_openList->getCapacity() < maxNodes)
    {
        if (m_openList)
        {
            m_openList->~dtNodeQueue();
            dtFree(m_openList);
            m_openList = 0;
        }
        m_openList = new (dtAlloc(sizeof(dtNodeQueue), DT_ALLOC_PERM)) dtNodeQueue(maxNodes);
        if (!m_openList)
            return DT_FAILURE | DT_OUT_OF_MEMORY;
    }
    else
    {
        m_openList->clear();
    }

    return DT_SUCCESS;
}

//////////////////////////////////////////////////////////////////////////////////////////
dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const
{
    dtAssert(m_nav);
    const dtMeshTile* tile = 0;
    const dtPoly* poly = 0;
    if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
        return DT_FAILURE | DT_INVALID_PARAM;
    if (!tile)
        return DT_FAILURE | DT_INVALID_PARAM;

    closestPointOnPolyInTile(tile, poly, pos, closest);

    return DT_SUCCESS;
}

void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly,
                                                  const float* pos, float* closest) const
{
    const unsigned int ip = (unsigned int)(poly - tile->polys);
    const dtPolyDetail* pd = &tile->detailMeshes[ip];

    // TODO: The commented out version finds 'cylinder distance' instead of 'sphere distance' to the navmesh.
    // Test and enable.
/*
    // Clamp point to be inside the polygon.
    float verts[DT_VERTS_PER_POLYGON*3];
    float edged[DT_VERTS_PER_POLYGON];
    float edget[DT_VERTS_PER_POLYGON];
    const int nv = poly->vertCount;
    for (int i = 0; i < nv; ++i)
        dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]);

    dtVcopy(closest, pos);
    if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
    {
        // Point is outside the polygon, dtClamp to nearest edge.
        float dmin = FLT_MAX;
        int imin = -1;
        for (int i = 0; i < nv; ++i)
        {
            if (edged[i] < dmin)
            {
                dmin = edged[i];
                imin = i;
            }
        }
        const float* va = &verts[imin*3];
        const float* vb = &verts[((imin+1)%nv)*3];
        dtVlerp(closest, va, vb, edget[imin]);
    }

    // Find height at the location.
    for (int j = 0; j < pd->triCount; ++j)
    {
        const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
        const float* v[3];
        for (int k = 0; k < 3; ++k)
        {
            if (t[k] < poly->vertCount)
                v[k] = &tile->verts[poly->verts[t[k]]*3];
            else
                v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
        }
        float h;
        if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
        {
            closest[1] = h;
            break;
        }
    }
*/
    float closestDistSqr = FLT_MAX;
    for (int j = 0; j < pd->triCount; ++j)
    {
        const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
        const float* v[3];
        for (int k = 0; k < 3; ++k)
        {
            if (t[k] < poly->vertCount)
                v[k] = &tile->verts[poly->verts[t[k]]*3];
            else
                v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
        }

        float pt[3];
        dtClosestPtPointTriangle(pt, pos, v[0], v[1], v[2]);
        float d = dtVdistSqr(pos, pt);

        if (d < closestDistSqr)
        {
            dtVcopy(closest, pt);
            closestDistSqr = d;
        }
    }
}

dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const
{
    dtAssert(m_nav);

    const dtMeshTile* tile = 0;
    const dtPoly* poly = 0;
    if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
        return DT_FAILURE | DT_INVALID_PARAM;

    // Collect vertices.
    float verts[DT_VERTS_PER_POLYGON*3];
    float edged[DT_VERTS_PER_POLYGON];
    float edget[DT_VERTS_PER_POLYGON];
    int nv = 0;
    for (int i = 0; i < (int)poly->vertCount; ++i)
    {
        dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
        nv++;
    }

    bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget);
    if (inside)
    {
        // Point is inside the polygon, return the point.
        dtVcopy(closest, pos);
    }
    else
    {
        // Point is outside the polygon, dtClamp to nearest edge.
        float dmin = FLT_MAX;
        int imin = -1;
        for (int i = 0; i < nv; ++i)
        {
            if (edged[i] < dmin)
            {
                dmin = edged[i];
                imin = i;
            }
        }
        const float* va = &verts[imin*3];
        const float* vb = &verts[((imin+1)%nv)*3];
        dtVlerp(closest, va, vb, edget[imin]);
    }

    return DT_SUCCESS;
}


dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* height) const
{
    dtAssert(m_nav);

    const dtMeshTile* tile = 0;
    const dtPoly* poly = 0;
    if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
        return DT_FAILURE | DT_INVALID_PARAM;

    if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
    {
        const float* v0 = &tile->verts[poly->verts[0]*3];
        const float* v1 = &tile->verts[poly->verts[1]*3];
        const float d0 = dtVdist(pos, v0);
        const float d1 = dtVdist(pos, v1);
        const float u = d0 / (d0+d1);
        if (height)
            *height = v0[1] + (v1[1] - v0[1]) * u;
        return DT_SUCCESS;
    }
    else
    {
        const unsigned int ip = (unsigned int)(poly - tile->polys);
        const dtPolyDetail* pd = &tile->detailMeshes[ip];
        for (int j = 0; j < pd->triCount; ++j)
        {
            const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
            const float* v[3];
            for (int k = 0; k < 3; ++k)
            {
                if (t[k] < poly->vertCount)
                    v[k] = &tile->verts[poly->verts[t[k]]*3];
                else
                    v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
            }
            float h;
            if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
            {
                if (height)
                    *height = h;
                return DT_SUCCESS;
            }
        }
    }

    return DT_FAILURE | DT_INVALID_PARAM;
}

dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* extents,
                                         const dtQueryFilter* filter,
                                         dtPolyRef* nearestRef, float* nearestPt) const
{
    dtAssert(m_nav);

    *nearestRef = 0;

    // Get nearby polygons from proximity grid.
    dtPolyRef polys[128];
    int polyCount = 0;
    if (dtStatusFailed(queryPolygons(center, extents, filter, polys, &polyCount, 128)))
        return DT_FAILURE | DT_INVALID_PARAM;

    // Find nearest polygon amongst the nearby polygons.
    dtPolyRef nearest = 0;
    float nearestDistanceSqr = FLT_MAX;
    for (int i = 0; i < polyCount; ++i)
    {
        dtPolyRef ref = polys[i];
        float closestPtPoly[3];
        closestPointOnPoly(ref, center, closestPtPoly);
        float d = dtVdistSqr(center, closestPtPoly);
        if (d < nearestDistanceSqr)
        {
            if (nearestPt)
                dtVcopy(nearestPt, closestPtPoly);
            nearestDistanceSqr = d;
            nearest = ref;
        }
    }

    if (nearestRef)
        *nearestRef = nearest;

    return DT_SUCCESS;
}

dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents,
                                                const dtQueryFilter* filter, float* nearestPt) const
{
    dtAssert(m_nav);

    float bmin[3], bmax[3];
    dtVsub(bmin, center, extents);
    dtVadd(bmax, center, extents);

    // Get nearby polygons from proximity grid.
    dtPolyRef polys[128];
    int polyCount = queryPolygonsInTile(tile, bmin, bmax, filter, polys, 128);

    // Find nearest polygon amongst the nearby polygons.
    dtPolyRef nearest = 0;
    float nearestDistanceSqr = FLT_MAX;
    for (int i = 0; i < polyCount; ++i)
    {
        dtPolyRef ref = polys[i];
        const dtPoly* poly = &tile->polys[m_nav->decodePolyIdPoly(ref)];
        float closestPtPoly[3];
        closestPointOnPolyInTile(tile, poly, center, closestPtPoly);

        float d = dtVdistSqr(center, closestPtPoly);
        if (d < nearestDistanceSqr)
        {
            if (nearestPt)
                dtVcopy(nearestPt, closestPtPoly);
            nearestDistanceSqr = d;
            nearest = ref;
        }
    }

    return nearest;
}

int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax,
                                        const dtQueryFilter* filter,
                                        dtPolyRef* polys, const int maxPolys) const
{
    dtAssert(m_nav);

    if (tile->bvTree)
    {
        const dtBVNode* node = &tile->bvTree[0];
        const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount];
        const float* tbmin = tile->header->bmin;
        const float* tbmax = tile->header->bmax;
        const float qfac = tile->header->bvQuantFactor;

        // Calculate quantized box
        unsigned short bmin[3], bmax[3];
        // dtClamp query box to world box.
        float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
        float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1];
        float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2];
        float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0];
        float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1];
        float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2];
        // Quantize
        bmin[0] = (unsigned short)(qfac * minx) & 0xfffe;
        bmin[1] = (unsigned short)(qfac * miny) & 0xfffe;
        bmin[2] = (unsigned short)(qfac * minz) & 0xfffe;
        bmax[0] = (unsigned short)(qfac * maxx + 1) | 1;
        bmax[1] = (unsigned short)(qfac * maxy + 1) | 1;
        bmax[2] = (unsigned short)(qfac * maxz + 1) | 1;

        // Traverse tree
        const dtPolyRef base = m_nav->getPolyRefBase(tile);
        int n = 0;
        while (node < end)
        {
            const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax);
            const bool isLeafNode = node->i >= 0;

            if (isLeafNode && overlap)
            {
                dtPolyRef ref = base | (dtPolyRef)node->i;
                if (filter->passFilter(ref, tile, &tile->polys[node->i]))
                {
                    if (n < maxPolys)
                        polys[n++] = ref;
                }
            }

            if (overlap || isLeafNode)
                node++;
            else
            {
                const int escapeIndex = -node->i;
                node += escapeIndex;
            }
        }

        return n;
    }
    else
    {
        float bmin[3], bmax[3];
        int n = 0;
        const dtPolyRef base = m_nav->getPolyRefBase(tile);
        for (int i = 0; i < tile->header->polyCount; ++i)
        {
            // Calc polygon bounds.
            dtPoly* p = &tile->polys[i];
            const float* v = &tile->verts[p->verts[0]*3];
            dtVcopy(bmin, v);
            dtVcopy(bmax, v);
            for (int j = 1; j < p->vertCount; ++j)
            {
                v = &tile->verts[p->verts[j]*3];
                dtVmin(bmin, v);
                dtVmax(bmax, v);
            }
            if (dtOverlapBounds(qmin,qmax, bmin,bmax))
            {
                const dtPolyRef ref = base | (dtPolyRef)i;
                if (filter->passFilter(ref, tile, p))
                {
                    if (n < maxPolys)
                        polys[n++] = ref;
                }
            }
        }
        return n;
    }
}

dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents,
                                       const dtQueryFilter* filter,
                                       dtPolyRef* polys, int* polyCount, const int maxPolys) const
{
    dtAssert(m_nav);

    float bmin[3], bmax[3];
    dtVsub(bmin, center, extents);
    dtVadd(bmax, center, extents);

    // Find tiles the query touches.
    int minx, miny, maxx, maxy;
    m_nav->calcTileLoc(bmin, &minx, &miny);
    m_nav->calcTileLoc(bmax, &maxx, &maxy);

    int n = 0;
    for (int y = miny; y <= maxy; ++y)
    {
        for (int x = minx; x <= maxx; ++x)
        {
            const dtMeshTile* tile = m_nav->getTileAt(x,y);
            if (!tile) continue;
            n += queryPolygonsInTile(tile, bmin, bmax, filter, polys+n, maxPolys-n);
            if (n >= maxPolys)
            {
                *polyCount = n;
                return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
            }
        }
    }
    *polyCount = n;

    return DT_SUCCESS;
}

dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
                                  const float* startPos, const float* endPos,
                                  const dtQueryFilter* filter,
                                  dtPolyRef* path, int* pathCount, const int maxPath) const
{
    dtAssert(m_nav);
    dtAssert(m_nodePool);
    dtAssert(m_openList);

    *pathCount = 0;

    if (!startRef || !endRef)
        return DT_FAILURE | DT_INVALID_PARAM;

    if (!maxPath)
        return DT_FAILURE | DT_INVALID_PARAM;

    // Validate input
    if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef))
        return DT_FAILURE | DT_INVALID_PARAM;

    if (startRef == endRef)
    {
        path[0] = startRef;
        *pathCount = 1;
        return DT_SUCCESS;
    }

    m_nodePool->clear();
    m_openList->clear();

    dtNode* startNode = m_nodePool->getNode(startRef);
    dtVcopy(startNode->pos, startPos);
    startNode->pidx = 0;
    startNode->cost = 0;
    startNode->total = dtVdist(startPos, endPos) * H_SCALE;
    startNode->id = startRef;
    startNode->flags = DT_NODE_OPEN;
    m_openList->push(startNode);

    dtNode* lastBestNode = startNode;
    float lastBestNodeCost = startNode->total;

    dtStatus status = DT_SUCCESS;

    while (!m_openList->empty())
    {
        // Remove node from open list and put it in closed list.
        dtNode* bestNode = m_openList->pop();
        bestNode->flags &= ~DT_NODE_OPEN;
        bestNode->flags |= DT_NODE_CLOSED;

        // Reached the goal, stop searching.
        if (bestNode->id == endRef)
        {
            lastBestNode = bestNode;
            break;
        }

        // Get current poly and tile.
        // The API input has been cheked already, skip checking internal data.
        const dtPolyRef bestRef = bestNode->id;
        const dtMeshTile* bestTile = 0;
        const dtPoly* bestPoly = 0;
        m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);

        // Get parent poly and tile.
        dtPolyRef parentRef = 0;
        const dtMeshTile* parentTile = 0;
        const dtPoly* parentPoly = 0;
        if (bestNode->pidx)
            parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
        if (parentRef)
            m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);

        for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
        {
            dtPolyRef neighbourRef = bestTile->links[i].ref;

            // Skip invalid ids and do not expand back to where we came from.
            if (!neighbourRef || neighbourRef == parentRef)
                continue;

            // Get neighbour poly and tile.
            // The API input has been cheked already, skip checking internal data.
            const dtMeshTile* neighbourTile = 0;
            const dtPoly* neighbourPoly = 0;
            m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);

            if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
                continue;

            dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
            if (!neighbourNode)
            {
                status |= DT_OUT_OF_NODES;
                continue;
            }

            // If the node is visited the first time, calculate node position.
            if (neighbourNode->flags == 0)
            {
                getEdgeMidPoint(bestRef, bestPoly, bestTile,
                                neighbourRef, neighbourPoly, neighbourTile,
                                neighbourNode->pos);
            }

            // Calculate cost and heuristic.
            float cost = 0;
            float heuristic = 0;

            // Special case for last node.
            if (neighbourRef == endRef)
            {
                // Cost
                const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos,
                                                      parentRef, parentTile, parentPoly,
                                                      bestRef, bestTile, bestPoly,
                                                      neighbourRef, neighbourTile, neighbourPoly);
                const float endCost = filter->getCost(neighbourNode->pos, endPos,
                                                      bestRef, bestTile, bestPoly,
                                                      neighbourRef, neighbourTile, neighbourPoly,
                                                      0, 0, 0);

                cost = bestNode->cost + curCost + endCost;
                heuristic = 0;
            }
            else
            {
                // Cost
                const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos,
                                                      parentRef, parentTile, parentPoly,
                                                      bestRef, bestTile, bestPoly,
                                                      neighbourRef, neighbourTile, neighbourPoly);
                cost = bestNode->cost + curCost;
                heuristic = dtVdist(neighbourNode->pos, endPos)*H_SCALE;
            }

            const float total = cost + heuristic;

            // The node is already in open list and the new result is worse, skip.
            if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
                continue;
            // The node is already visited and process, and the new result is worse, skip.
            if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total)
                continue;

            // Add or update the node.
            neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
            neighbourNode->id = neighbourRef;
            neighbourNode->flags &= ~DT_NODE_CLOSED;
            neighbourNode->cost = cost;
            neighbourNode->total = total;

            if (neighbourNode->flags & DT_NODE_OPEN)
            {
                // Already in open, update node location.
                m_openList->modify(neighbourNode);
            }
            else
            {
                // Put the node in open list.
                neighbourNode->flags |= DT_NODE_OPEN;
                m_openList->push(neighbourNode);
            }

            // Update nearest node to target so far.
            if (heuristic < lastBestNodeCost)
            {
                lastBestNodeCost = heuristic;
                lastBestNode = neighbourNode;
            }
        }
    }

    if (lastBestNode->id != endRef)
        status |= DT_PARTIAL_RESULT;

    // Reverse the path.
    dtNode* prev = 0;
    dtNode* node = lastBestNode;
    do
    {
        dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
        node->pidx = m_nodePool->getNodeIdx(prev);
        prev = node;
        node = next;
    }
    while (node);

    // Store path
    node = prev;
    int n = 0;
    do
    {
        path[n++] = node->id;
        if (n >= maxPath)
        {
            status |= DT_BUFFER_TOO_SMALL;
            break;
        }
        node = m_nodePool->getNodeAtIdx(node->pidx);
    }
    while (node);

    *pathCount = n;

    return status;
}

dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef,
                                            const float* startPos, const float* endPos,
                                            const dtQueryFilter* filter)
{
    dtAssert(m_nav);
    dtAssert(m_nodePool);
    dtAssert(m_openList);

    // Init path state.
    memset(&m_query, 0, sizeof(dtQueryData));
    m_query.status = DT_FAILURE;
    m_query.startRef = startRef;
    m_query.endRef = endRef;
    dtVcopy(m_query.startPos, startPos);
    dtVcopy(m_query.endPos, endPos);
    m_query.filter = filter;

    if (!startRef || !endRef)
        return DT_FAILURE | DT_INVALID_PARAM;

    // Validate input
    if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef))
        return DT_FAILURE | DT_INVALID_PARAM;

    if (startRef == endRef)
    {
        m_query.status = DT_SUCCESS;
        return DT_SUCCESS;
    }

    m_nodePool->clear();
    m_openList->clear();

    dtNode* startNode = m_nodePool->getNode(startRef);
    dtVcopy(startNode->pos, startPos);
    startNode->pidx = 0;
    startNode->cost = 0;
    startNode->total = dtVdist(startPos, endPos) * H_SCALE;
    startNode->id = startRef;
    startNode->flags = DT_NODE_OPEN;
    m_openList->push(startNode);

    m_query.status = DT_IN_PROGRESS;
    m_query.lastBestNode = startNode;
    m_query.lastBestNodeCost = startNode->total;

    return m_query.status;
}

dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
{
    if (!dtStatusInProgress(m_query.status))
        return m_query.status;

    // Make sure the request is still valid.
    if (!m_nav->isValidPolyRef(m_query.startRef) || !m_nav->isValidPolyRef(m_query.endRef))
    {
        m_query.status = DT_FAILURE;
        return DT_FAILURE;
    }

    int iter = 0;
    while (iter < maxIter && !m_openList->empty())
    {
        iter++;

        // Remove node from open list and put it in closed list.
        dtNode* bestNode = m_openList->pop();
        bestNode->flags &= ~DT_NODE_OPEN;
        bestNode->flags |= DT_NODE_CLOSED;

        // Reached the goal, stop searching.
        if (bestNode->id == m_query.endRef)
        {
            m_query.lastBestNode = bestNode;
            const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
            m_query.status = DT_SUCCESS | details;
            return m_query.status;
        }

        // Get current poly and tile.
        // The API input has been cheked already, skip checking internal data.
        const dtPolyRef bestRef = bestNode->id;
        const dtMeshTile* bestTile = 0;
        const dtPoly* bestPoly = 0;
        if (dtStatusFailed(m_nav->getTileAndPolyByRef(bestRef, &bestTile, &bestPoly)))
        {
            // The polygon has disappeared during the sliced query, fail.
            m_query.status = DT_FAILURE;
            return m_query.status;
        }

        // Get parent poly and tile.
        dtPolyRef parentRef = 0;
        const dtMeshTile* parentTile = 0;
        const dtPoly* parentPoly = 0;
        if (bestNode->pidx)
            parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
        if (parentRef)
        {
            if (dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly)))
            {
                // The polygon has disappeared during the sliced query, fail.
                m_query.status = DT_FAILURE;
                return m_query.status;
            }
        }

        for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
        {
            dtPolyRef neighbourRef = bestTile->links[i].ref;

            // Skip invalid ids and do not expand back to where we came from.
            if (!neighbourRef || neighbourRef == parentRef)
                continue;

            // Get neighbour poly and tile.
            // The API input has been cheked already, skip checking internal data.
            const dtMeshTile* neighbourTile = 0;
            const dtPoly* neighbourPoly = 0;
            m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);

            if (!m_query.filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
                continue;

            dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
            if (!neighbourNode)
            {
                m_query.status |= DT_OUT_OF_NODES;
                continue;
            }

            // If the node is visited the first time, calculate node position.
            if (neighbourNode->flags == 0)
            {
                getEdgeMidPoint(bestRef, bestPoly, bestTile,
                                neighbourRef, neighbourPoly, neighbourTile,
                                neighbourNode->pos);
            }

            // Calculate cost and heuristic.
            float cost = 0;
            float heuristic = 0;

            // Special case for last node.
            if (neighbourRef == m_query.endRef)
            {
                // Cost
                const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos,
                                                              parentRef, parentTile, parentPoly,
                                                              bestRef, bestTile, bestPoly,
                                                              neighbourRef, neighbourTile, neighbourPoly);
                const float endCost = m_query.filter->getCost(neighbourNode->pos, m_query.endPos,
                                                              bestRef, bestTile, bestPoly,
                                                              neighbourRef, neighbourTile, neighbourPoly,
                                                              0, 0, 0);

                cost = bestNode->cost + curCost + endCost;
                heuristic = 0;
            }
            else
            {
                // Cost
                const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos,
                                                              parentRef, parentTile, parentPoly,
                                                              bestRef, bestTile, bestPoly,
                                                              neighbourRef, neighbourTile, neighbourPoly);
                cost = bestNode->cost + curCost;
                heuristic = dtVdist(neighbourNode->pos, m_query.endPos)*H_SCALE;
            }

            const float total = cost + heuristic;

            // The node is already in open list and the new result is worse, skip.
            if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
                continue;
            // The node is already visited and process, and the new result is worse, skip.
            if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total)
                continue;

            // Add or update the node.
            neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
            neighbourNode->id = neighbourRef;
            neighbourNode->flags &= ~DT_NODE_CLOSED;
            neighbourNode->cost = cost;
            neighbourNode->total = total;

            if (neighbourNode->flags & DT_NODE_OPEN)
            {
                // Already in open, update node location.
                m_openList->modify(neighbourNode);
            }
            else
            {
                // Put the node in open list.
                neighbourNode->flags |= DT_NODE_OPEN;
                m_openList->push(neighbourNode);
            }

            // Update nearest node to target so far.
            if (heuristic < m_query.lastBestNodeCost)
            {
                m_query.lastBestNodeCost = heuristic;
                m_query.lastBestNode = neighbourNode;
            }
        }
    }

    // Exhausted all nodes, but could not find path.
    if (m_openList->empty())
    {
        const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
        m_query.status = DT_SUCCESS | details;
    }

    return m_query.status;
}

dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath)
{
    *pathCount = 0;

    if (dtStatusFailed(m_query.status))
    {
        // Reset query.
        memset(&m_query, 0, sizeof(dtQueryData));
        return DT_FAILURE;
    }

    int n = 0;

    if (m_query.startRef == m_query.endRef)
    {
        // Special case: the search starts and ends at same poly.
        path[n++] = m_query.startRef;
    }
    else
    {
        // Reverse the path.
        dtAssert(m_query.lastBestNode);

        if (m_query.lastBestNode->id != m_query.endRef)
            m_query.status |= DT_PARTIAL_RESULT;

        dtNode* prev = 0;
        dtNode* node = m_query.lastBestNode;
        do
        {
            dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
            node->pidx = m_nodePool->getNodeIdx(prev);
            prev = node;
            node = next;
        }
        while (node);

        // Store path
        node = prev;
        do
        {
            path[n++] = node->id;
            if (n >= maxPath)
            {
                m_query.status |= DT_BUFFER_TOO_SMALL;
                break;
            }
            node = m_nodePool->getNodeAtIdx(node->pidx);
        }
        while (node);
    }

    const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;

    // Reset query.
    memset(&m_query, 0, sizeof(dtQueryData));

    *pathCount = n;

    return DT_SUCCESS | details;
}

dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize,
                                                       dtPolyRef* path, int* pathCount, const int maxPath)
{
    *pathCount = 0;

    if (existingSize == 0)
    {
        return DT_FAILURE;
    }

    if (dtStatusFailed(m_query.status))
    {
        // Reset query.
        memset(&m_query, 0, sizeof(dtQueryData));
        return DT_FAILURE;
    }

    int n = 0;

    if (m_query.startRef == m_query.endRef)
    {
        // Special case: the search starts and ends at same poly.
        path[n++] = m_query.startRef;
    }
    else
    {
        // Find furthest existing node that was visited.
        dtNode* prev = 0;
        dtNode* node = 0;
        for (int i = existingSize-1; i >= 0; --i)
        {
            node = m_nodePool->findNode(existing[i]);
            if (node)
                break;
        }

        if (!node)
        {
            return DT_FAILURE;
        }

        // Reverse the path.
        do
        {
            dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
            node->pidx = m_nodePool->getNodeIdx(prev);
            prev = node;
            node = next;
        }
        while (node);

        // Store path
        node = prev;
        do
        {
            path[n++] = node->id;
            if (n >= maxPath)
            {
                m_query.status |= DT_BUFFER_TOO_SMALL;
                break;
            }
            node = m_nodePool->getNodeAtIdx(node->pidx);
        }
        while (node);
    }

    const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;

    // Reset query.
    memset(&m_query, 0, sizeof(dtQueryData));

    *pathCount = n;

    return DT_SUCCESS | details;
}


dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* endPos,
                                          const dtPolyRef* path, const int pathSize,
                                          float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
                                          int* straightPathCount, const int maxStraightPath) const
{
    dtAssert(m_nav);

    *straightPathCount = 0;

    if (!maxStraightPath)
        return DT_FAILURE | DT_INVALID_PARAM;

    if (!path[0])
        return DT_FAILURE | DT_INVALID_PARAM;

    int n = 0;

    // TODO: Should this be callers responsibility?
    float closestStartPos[3];
    if (dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos)))
        return DT_FAILURE | DT_INVALID_PARAM;

    // Add start point.
    dtVcopy(&straightPath[n*3], closestStartPos);
    if (straightPathFlags)
        straightPathFlags[n] = DT_STRAIGHTPATH_START;
    if (straightPathRefs)
        straightPathRefs[n] = path[0];
    n++;
    if (n >= maxStraightPath)
    {
        *straightPathCount = n;
        return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
    }

    float closestEndPos[3];
    if (dtStatusFailed(closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos)))
        return DT_FAILURE | DT_INVALID_PARAM;

    if (pathSize > 1)
    {
        float portalApex[3], portalLeft[3], portalRight[3];
        dtVcopy(portalApex, closestStartPos);
        dtVcopy(portalLeft, portalApex);
        dtVcopy(portalRight, portalApex);
        int apexIndex = 0;
        int leftIndex = 0;
        int rightIndex = 0;

        unsigned char leftPolyType = 0;
        unsigned char rightPolyType = 0;

        dtPolyRef leftPolyRef = path[0];
        dtPolyRef rightPolyRef = path[0];

        for (int i = 0; i < pathSize; ++i)
        {
            float left[3], right[3];
            unsigned char fromType, toType;

            if (i+1 < pathSize)
            {
                // Next portal.
                if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType)))
                {
                    if (dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos)))
                        return DT_FAILURE | DT_INVALID_PARAM;

                    dtVcopy(&straightPath[n*3], closestEndPos);
                    if (straightPathFlags)
                        straightPathFlags[n] = 0;
                    if (straightPathRefs)
                        straightPathRefs[n] = path[i];
                    n++;

                    return DT_SUCCESS;
                }

                // If starting really close the portal, advance.
                if (i == 0)
                {
                    float t;
                    if (dtDistancePtSegSqr2D(portalApex, left, right, t) < dtSqr(0.001f))
                        continue;
                }
            }
            else
            {
                // End of the path.
                dtVcopy(left, closestEndPos);
                dtVcopy(right, closestEndPos);

                fromType = toType = DT_POLYTYPE_GROUND;
            }

            // Right vertex.
            if (dtTriArea2D(portalApex, portalRight, right) <= 0.0f)
            {
                if (dtVequal(portalApex, portalRight) || dtTriArea2D(portalApex, portalLeft, right) > 0.0f)
                {
                    dtVcopy(portalRight, right);
                    rightPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
                    rightPolyType = toType;
                    rightIndex = i;
                }
                else
                {
                    dtVcopy(portalApex, portalLeft);
                    apexIndex = leftIndex;

                    unsigned char flags = 0;
                    if (!leftPolyRef)
                        flags = DT_STRAIGHTPATH_END;
                    else if (leftPolyType == DT_POLYTYPE_OFFMESH_CONNECTION)
                        flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
                    dtPolyRef ref = leftPolyRef;

                    if (!dtVequal(&straightPath[(n-1)*3], portalApex))
                    {
                        // Append new vertex.
                        dtVcopy(&straightPath[n*3], portalApex);
                        if (straightPathFlags)
                            straightPathFlags[n] = flags;
                        if (straightPathRefs)
                            straightPathRefs[n] = ref;
                        n++;
                        // If reached end of path or there is no space to append more vertices, return.
                        if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath)
                        {
                            *straightPathCount = n;
                            return DT_SUCCESS | ((n >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
                        }
                    }
                    else
                    {
                        // The vertices are equal, update flags and poly.
                        if (straightPathFlags)
                            straightPathFlags[n-1] = flags;
                        if (straightPathRefs)
                            straightPathRefs[n-1] = ref;
                    }

                    dtVcopy(portalLeft, portalApex);
                    dtVcopy(portalRight, portalApex);
                    leftIndex = apexIndex;
                    rightIndex = apexIndex;

                    // Restart
                    i = apexIndex;

                    continue;
                }
            }

            // Left vertex.
            if (dtTriArea2D(portalApex, portalLeft, left) >= 0.0f)
            {
                if (dtVequal(portalApex, portalLeft) || dtTriArea2D(portalApex, portalRight, left) < 0.0f)
                {
                    dtVcopy(portalLeft, left);
                    leftPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
                    leftPolyType = toType;
                    leftIndex = i;
                }
                else
                {
                    dtVcopy(portalApex, portalRight);
                    apexIndex = rightIndex;

                    unsigned char flags = 0;
                    if (!rightPolyRef)
                        flags = DT_STRAIGHTPATH_END;
                    else if (rightPolyType == DT_POLYTYPE_OFFMESH_CONNECTION)
                        flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
                    dtPolyRef ref = rightPolyRef;

                    if (!dtVequal(&straightPath[(n-1)*3], portalApex))
                    {
                        // Append new vertex.
                        dtVcopy(&straightPath[n*3], portalApex);
                        if (straightPathFlags)
                            straightPathFlags[n] = flags;
                        if (straightPathRefs)
                            straightPathRefs[n] = ref;
                        n++;
                        // If reached end of path or there is no space to append more vertices, return.
                        if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath)
                        {
                            *straightPathCount = n;
                            return DT_SUCCESS | ((n >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
                        }
                    }
                    else
                    {
                        // The vertices are equal, update flags and poly.
                        if (straightPathFlags)
                            straightPathFlags[n-1] = flags;
                        if (straightPathRefs)
                            straightPathRefs[n-1] = ref;
                    }

                    dtVcopy(portalLeft, portalApex);
                    dtVcopy(portalRight, portalApex);
                    leftIndex = apexIndex;
                    rightIndex = apexIndex;

                    // Restart
                    i = apexIndex;

                    continue;
                }
            }
        }
    }

    // If the point already exists, remove it and add reappend the actual end location.
    if (n > 0 && dtVequal(&straightPath[(n-1)*3], closestEndPos))
        n--;

    // Add end point.
    if (n < maxStraightPath)
    {
        dtVcopy(&straightPath[n*3], closestEndPos);
        if (straightPathFlags)
            straightPathFlags[n] = DT_STRAIGHTPATH_END;
        if (straightPathRefs)
            straightPathRefs[n] = 0;
        n++;
    }

    *straightPathCount = n;

    return DT_SUCCESS | ((n >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
}

dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos,
                                          const dtQueryFilter* filter,
                                          float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const
{
    dtAssert(m_nav);
    dtAssert(m_tinyNodePool);

    *visitedCount = 0;

    // Validate input
    if (!startRef)
        return DT_FAILURE | DT_INVALID_PARAM;
    if (!m_nav->isValidPolyRef(startRef))
        return DT_FAILURE | DT_INVALID_PARAM;

    dtStatus status = DT_SUCCESS;

    static const int MAX_STACK = 48;
    dtNode* stack[MAX_STACK];
    int nstack = 0;

    m_tinyNodePool->clear();

    dtNode* startNode = m_tinyNodePool->getNode(startRef);
    startNode->pidx = 0;
    startNode->cost = 0;
    startNode->total = 0;
    startNode->id = startRef;
    startNode->flags = DT_NODE_CLOSED;
    stack[nstack++] = startNode;

    float bestPos[3];
    float bestDist = FLT_MAX;
    dtNode* bestNode = 0;
    dtVcopy(bestPos, startPos);

    // Search constraints
    float searchPos[3], searchRadSqr;
    dtVlerp(searchPos, startPos, endPos, 0.5f);
    searchRadSqr = dtSqr(dtVdist(startPos, endPos)/2.0f + 0.001f);

    float verts[DT_VERTS_PER_POLYGON*3];

    while (nstack)
    {
        // Pop front.
        dtNode* curNode = stack[0];
        for (int i = 0; i < nstack-1; ++i)
            stack[i] = stack[i+1];
        nstack--;

        // Get poly and tile.
        // The API input has been cheked already, skip checking internal data.
        const dtPolyRef curRef = curNode->id;
        const dtMeshTile* curTile = 0;
        const dtPoly* curPoly = 0;
        m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);

        // Collect vertices.
        const int nverts = curPoly->vertCount;
        for (int i = 0; i < nverts; ++i)
            dtVcopy(&verts[i*3], &curTile->verts[curPoly->verts[i]*3]);

        // If target is inside the poly, stop search.
        if (dtPointInPolygon(endPos, verts, nverts))
        {
            bestNode = curNode;
            dtVcopy(bestPos, endPos);
            break;
        }

        // Find wall edges and find nearest point inside the walls.
        for (int i = 0, j = (int)curPoly->vertCount-1; i < (int)curPoly->vertCount; j = i++)
        {
            // Find links to neighbours.
            static const int MAX_NEIS = 8;
            int nneis = 0;
            dtPolyRef neis[MAX_NEIS];

            if (curPoly->neis[j] & DT_EXT_LINK)
            {
                // Tile border.
                for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next)
                {
                    const dtLink* link = &curTile->links[k];
                    if (link->edge == j)
                    {
                        if (link->ref != 0)
                        {
                            const dtMeshTile* neiTile = 0;
                            const dtPoly* neiPoly = 0;
                            m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
                            if (filter->passFilter(link->ref, neiTile, neiPoly))
                            {
                                if (nneis < MAX_NEIS)
                                    neis[nneis++] = link->ref;
                            }
                        }
                    }
                }
            }
            else if (curPoly->neis[j])
            {
                const unsigned int idx = (unsigned int)(curPoly->neis[j]-1);
                const dtPolyRef ref = m_nav->getPolyRefBase(curTile) | idx;
                if (filter->passFilter(ref, curTile, &curTile->polys[idx]))
                {
                    // Internal edge, encode id.
                    neis[nneis++] = ref;
                }
            }

            if (!nneis)
            {
                // Wall edge, calc distance.
                const float* vj = &verts[j*3];
                const float* vi = &verts[i*3];
                float tseg;
                const float distSqr = dtDistancePtSegSqr2D(endPos, vj, vi, tseg);
                if (distSqr < bestDist)
                {
                    // Update nearest distance.
                    dtVlerp(bestPos, vj,vi, tseg);
                    bestDist = distSqr;
                    bestNode = curNode;
                }
            }
            else
            {
                for (int k = 0; k < nneis; ++k)
                {
                    // Skip if no node can be allocated.
                    dtNode* neighbourNode = m_tinyNodePool->getNode(neis[k]);
                    if (!neighbourNode)
                        continue;
                    // Skip if already visited.
                    if (neighbourNode->flags & DT_NODE_CLOSED)
                        continue;

                    // Skip the link if it is too far from search constraint.
                    // TODO: Maybe should use getPortalPoints(), but this one is way faster.
                    const float* vj = &verts[j*3];
                    const float* vi = &verts[i*3];
                    float tseg;
                    float distSqr = dtDistancePtSegSqr2D(searchPos, vj, vi, tseg);
                    if (distSqr > searchRadSqr)
                        continue;

                    // Mark as the node as visited and push to queue.
                    if (nstack < MAX_STACK)
                    {
                        neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode);
                        neighbourNode->flags |= DT_NODE_CLOSED;
                        stack[nstack++] = neighbourNode;
                    }
                }
            }
        }
    }

    int n = 0;
    if (bestNode)
    {
        // Reverse the path.
        dtNode* prev = 0;
        dtNode* node = bestNode;
        do
        {
            dtNode* next = m_tinyNodePool->getNodeAtIdx(node->pidx);
            node->pidx = m_tinyNodePool->getNodeIdx(prev);
            prev = node;
            node = next;
        }
        while (node);

        // Store result
        node = prev;
        do
        {
            visited[n++] = node->id;
            if (n >= maxVisitedSize)
            {
                status |= DT_BUFFER_TOO_SMALL;
                break;
            }
            node = m_tinyNodePool->getNodeAtIdx(node->pidx);
        }
        while (node);
    }

    dtVcopy(resultPos, bestPos);

    *visitedCount = n;

    return status;
}


dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right,
                                         unsigned char& fromType, unsigned char& toType) const
{
    dtAssert(m_nav);

    const dtMeshTile* fromTile = 0;
    const dtPoly* fromPoly = 0;
    if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly)))
        return DT_FAILURE | DT_INVALID_PARAM;
    fromType = fromPoly->getType();

    const dtMeshTile* toTile = 0;
    const dtPoly* toPoly = 0;
    if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly)))
        return DT_FAILURE | DT_INVALID_PARAM;
    toType = toPoly->getType();

    return getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right);
}

// Returns portal points between two polygons.
dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile,
                                         dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
                                         float* left, float* right) const
{
    // Find the link that points to the 'to' polygon.
    const dtLink* link = 0;
    for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next)
    {
        if (fromTile->links[i].ref == to)
        {
            link = &fromTile->links[i];
            break;
        }
    }
    if (!link)
        return DT_FAILURE | DT_INVALID_PARAM;

    // Handle off-mesh connections.
    if (fromPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
    {
        // Find link that points to first vertex.
        for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next)
        {
            if (fromTile->links[i].ref == to)
            {
                const int v = fromTile->links[i].edge;
                dtVcopy(left, &fromTile->verts[fromPoly->verts[v]*3]);
                dtVcopy(right, &fromTile->verts[fromPoly->verts[v]*3]);
                return DT_SUCCESS;
            }
        }
        return DT_FAILURE | DT_INVALID_PARAM;
    }

    if (toPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
    {
        for (unsigned int i = toPoly->firstLink; i != DT_NULL_LINK; i = toTile->links[i].next)
        {
            if (toTile->links[i].ref == from)
            {
                const int v = toTile->links[i].edge;
                dtVcopy(left, &toTile->verts[toPoly->verts[v]*3]);
                dtVcopy(right, &toTile->verts[toPoly->verts[v]*3]);
                return DT_SUCCESS;
            }
        }
        return DT_FAILURE | DT_INVALID_PARAM;
    }

    // Find portal vertices.
    const int v0 = fromPoly->verts[link->edge];
    const int v1 = fromPoly->verts[(link->edge+1) % (int)fromPoly->vertCount];
    dtVcopy(left, &fromTile->verts[v0*3]);
    dtVcopy(right, &fromTile->verts[v1*3]);

    // If the link is at tile boundary, dtClamp the vertices to
    // the link width.
    if (link->side != 0xff)
    {
        // Unpack portal limits.
        if (link->bmin != 0 || link->bmax != 255)
        {
            const float s = 1.0f/255.0f;
            const float tmin = link->bmin*s;
            const float tmax = link->bmax*s;
            dtVlerp(left, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmin);
            dtVlerp(right, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmax);
        }
    }

    return DT_SUCCESS;
}

// Returns edge mid point between two polygons.
dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const
{
    float left[3], right[3];
    unsigned char fromType, toType;
    if (dtStatusFailed(getPortalPoints(from, to, left,right, fromType, toType)))
        return DT_FAILURE | DT_INVALID_PARAM;
    mid[0] = (left[0]+right[0])*0.5f;
    mid[1] = (left[1]+right[1])*0.5f;
    mid[2] = (left[2]+right[2])*0.5f;
    return DT_SUCCESS;
}

dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile,
                                         dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
                                         float* mid) const
{
    float left[3], right[3];
    if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
        return DT_FAILURE | DT_INVALID_PARAM;
    mid[0] = (left[0]+right[0])*0.5f;
    mid[1] = (left[1]+right[1])*0.5f;
    mid[2] = (left[2]+right[2])*0.5f;
    return DT_SUCCESS;
}

dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
                                 const dtQueryFilter* filter,
                                 float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const
{
    dtAssert(m_nav);

    *t = 0;
    if (pathCount)
        *pathCount = 0;

    // Validate input
    if (!startRef || !m_nav->isValidPolyRef(startRef))
        return DT_FAILURE | DT_INVALID_PARAM;

    dtPolyRef curRef = startRef;
    float verts[DT_VERTS_PER_POLYGON*3];
    int n = 0;

    hitNormal[0] = 0;
    hitNormal[1] = 0;
    hitNormal[2] = 0;

    dtStatus status = DT_SUCCESS;

    while (curRef)
    {
        // Cast ray against current polygon.

        // The API input has been cheked already, skip checking internal data.
        const dtMeshTile* tile = 0;
        const dtPoly* poly = 0;
        m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly);

        // Collect vertices.
        int nv = 0;
        for (int i = 0; i < (int)poly->vertCount; ++i)
        {
            dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
            nv++;
        }

        float tmin, tmax;
        int segMin, segMax;
        if (!dtIntersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax))
        {
            // Could not hit the polygon, keep the old t and report hit.
            if (pathCount)
                *pathCount = n;
            return status;
        }
        // Keep track of furthest t so far.
        if (tmax > *t)
            *t = tmax;

        // Store visited polygons.
        if (n < maxPath)
            path[n++] = curRef;
        else
            status |= DT_BUFFER_TOO_SMALL;

        // Ray end is completely inside the polygon.
        if (segMax == -1)
        {
            *t = FLT_MAX;
            if (pathCount)
                *pathCount = n;
            return status;
        }

        // Follow neighbours.
        dtPolyRef nextRef = 0;

        for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next)
        {
            const dtLink* link = &tile->links[i];

            // Find link which contains this edge.
            if ((int)link->edge != segMax)
                continue;

            // Get pointer to the next polygon.
            const dtMeshTile* nextTile = 0;
            const dtPoly* nextPoly = 0;
            m_nav->getTileAndPolyByRefUnsafe(link->ref, &nextTile, &nextPoly);

            // Skip off-mesh connections.
            if (nextPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
                continue;

            // Skip links based on filter.
            if (!filter->passFilter(link->ref, nextTile, nextPoly))
                continue;

            // If the link is internal, just return the ref.
            if (link->side == 0xff)
            {
                nextRef = link->ref;
                break;
            }

            // If the link is at tile boundary,

            // Check if the link spans the whole edge, and accept.
            if (link->bmin == 0 && link->bmax == 255)
            {
                nextRef = link->ref;
                break;
            }

            // Check for partial edge links.
            const int v0 = poly->verts[link->edge];
            const int v1 = poly->verts[(link->edge+1) % poly->vertCount];
            const float* left = &tile->verts[v0*3];
            const float* right = &tile->verts[v1*3];

            // Check that the intersection lies inside the link portal.
            if (link->side == 0 || link->side == 4)
            {
                // Calculate link size.
                const float s = 1.0f/255.0f;
                float lmin = left[2] + (right[2] - left[2])*(link->bmin*s);
                float lmax = left[2] + (right[2] - left[2])*(link->bmax*s);
                if (lmin > lmax) dtSwap(lmin, lmax);

                // Find Z intersection.
                float z = startPos[2] + (endPos[2]-startPos[2])*tmax;
                if (z >= lmin && z <= lmax)
                {
                    nextRef = link->ref;
                    break;
                }
            }
            else if (link->side == 2 || link->side == 6)
            {
                // Calculate link size.
                const float s = 1.0f/255.0f;
                float lmin = left[0] + (right[0] - left[0])*(link->bmin*s);
                float lmax = left[0] + (right[0] - left[0])*(link->bmax*s);
                if (lmin > lmax) dtSwap(lmin, lmax);

                // Find X intersection.
                float x = startPos[0] + (endPos[0]-startPos[0])*tmax;
                if (x >= lmin && x <= lmax)
                {
                    nextRef = link->ref;
                    break;
                }
            }
        }

        if (!nextRef)
        {
            // No neighbour, we hit a wall.

            // Calculate hit normal.
            const int a = segMax;
            const int b = segMax+1 < nv ? segMax+1 : 0;
            const float* va = &verts[a*3];
            const float* vb = &verts[b*3];
            const float dx = vb[0] - va[0];
            const float dz = vb[2] - va[2];
            hitNormal[0] = dz;
            hitNormal[1] = 0;
            hitNormal[2] = -dx;
            dtVnormalize(hitNormal);

            if (pathCount)
                *pathCount = n;
            return status;
        }

        // No hit, advance to neighbour polygon.
        curRef = nextRef;
    }

    if (pathCount)
        *pathCount = n;

    return status;
}

dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius,
                                               const dtQueryFilter* filter,
                                               dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
                                               int* resultCount, const int maxResult) const
{
    dtAssert(m_nav);
    dtAssert(m_nodePool);
    dtAssert(m_openList);

    *resultCount = 0;

    // Validate input
    if (!startRef || !m_nav->isValidPolyRef(startRef))
        return DT_FAILURE | DT_INVALID_PARAM;

    m_nodePool->clear();
    m_openList->clear();

    dtNode* startNode = m_nodePool->getNode(startRef);
    dtVcopy(startNode->pos, centerPos);
    startNode->pidx = 0;
    startNode->cost = 0;
    startNode->total = 0;
    startNode->id = startRef;
    startNode->flags = DT_NODE_OPEN;
    m_openList->push(startNode);

    dtStatus status = DT_SUCCESS;

    int n = 0;
    if (n < maxResult)
    {
        if (resultRef)
            resultRef[n] = startNode->id;
        if (resultParent)
            resultParent[n] = 0;
        if (resultCost)
            resultCost[n] = 0;
        ++n;
    }
    else
    {
        status |= DT_BUFFER_TOO_SMALL;
    }

    const float radiusSqr = dtSqr(radius);

    while (!m_openList->empty())
    {
        dtNode* bestNode = m_openList->pop();
        bestNode->flags &= ~DT_NODE_OPEN;
        bestNode->flags |= DT_NODE_CLOSED;

        // Get poly and tile.
        // The API input has been cheked already, skip checking internal data.
        const dtPolyRef bestRef = bestNode->id;
        const dtMeshTile* bestTile = 0;
        const dtPoly* bestPoly = 0;
        m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);

        // Get parent poly and tile.
        dtPolyRef parentRef = 0;
        const dtMeshTile* parentTile = 0;
        const dtPoly* parentPoly = 0;
        if (bestNode->pidx)
            parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
        if (parentRef)
            m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);

        for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
        {
            const dtLink* link = &bestTile->links[i];
            dtPolyRef neighbourRef = link->ref;
            // Skip invalid neighbours and do not follow back to parent.
            if (!neighbourRef || neighbourRef == parentRef)
                continue;

            // Expand to neighbour
            const dtMeshTile* neighbourTile = 0;
            const dtPoly* neighbourPoly = 0;
            m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);

            // Do not advance if the polygon is excluded by the filter.
            if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
                continue;

            // Find edge and calc distance to the edge.
            float va[3], vb[3];
            if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
                continue;

            // If the circle is not touching the next polygon, skip it.
            float tseg;
            float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
            if (distSqr > radiusSqr)
                continue;

            dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
            if (!neighbourNode)
            {
                status |= DT_OUT_OF_NODES;
                continue;
            }

            if (neighbourNode->flags & DT_NODE_CLOSED)
                continue;

            // Cost
            if (neighbourNode->flags == 0)
                dtVlerp(neighbourNode->pos, va, vb, 0.5f);

            const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);

            // The node is already in open list and the new result is worse, skip.
            if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
                continue;

            neighbourNode->id = neighbourRef;
            neighbourNode->flags &= ~DT_NODE_CLOSED;
            neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
            neighbourNode->total = total;

            if (neighbourNode->flags & DT_NODE_OPEN)
            {
                m_openList->modify(neighbourNode);
            }
            else
            {
                if (n < maxResult)
                {
                    if (resultRef)
                        resultRef[n] = neighbourNode->id;
                    if (resultParent)
                        resultParent[n] = m_nodePool->getNodeAtIdx(neighbourNode->pidx)->id;
                    if (resultCost)
                        resultCost[n] = neighbourNode->total;
                    ++n;
                }
                else
                {
                    status |= DT_BUFFER_TOO_SMALL;
                }
                neighbourNode->flags = DT_NODE_OPEN;
                m_openList->push(neighbourNode);
            }
        }
    }

    *resultCount = n;

    return status;
}

dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* verts, const int nverts,
                                              const dtQueryFilter* filter,
                                              dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
                                              int* resultCount, const int maxResult) const
{
    dtAssert(m_nav);
    dtAssert(m_nodePool);
    dtAssert(m_openList);

    *resultCount = 0;

    // Validate input
    if (!startRef || !m_nav->isValidPolyRef(startRef))
        return DT_FAILURE | DT_INVALID_PARAM;

    m_nodePool->clear();
    m_openList->clear();

    float centerPos[3] = {0,0,0};
    for (int i = 0; i < nverts; ++i)
        dtVadd(centerPos,centerPos,&verts[i*3]);
    dtVscale(centerPos,centerPos,1.0f/nverts);

    dtNode* startNode = m_nodePool->getNode(startRef);
    dtVcopy(startNode->pos, centerPos);
    startNode->pidx = 0;
    startNode->cost = 0;
    startNode->total = 0;
    startNode->id = startRef;
    startNode->flags = DT_NODE_OPEN;
    m_openList->push(startNode);

    dtStatus status = DT_SUCCESS;

    int n = 0;
    if (n < maxResult)
    {
        if (resultRef)
            resultRef[n] = startNode->id;
        if (resultParent)
            resultParent[n] = 0;
        if (resultCost)
            resultCost[n] = 0;
        ++n;
    }
    else
    {
        status |= DT_BUFFER_TOO_SMALL;
    }

    while (!m_openList->empty())
    {
        dtNode* bestNode = m_openList->pop();
        bestNode->flags &= ~DT_NODE_OPEN;
        bestNode->flags |= DT_NODE_CLOSED;

        // Get poly and tile.
        // The API input has been cheked already, skip checking internal data.
        const dtPolyRef bestRef = bestNode->id;
        const dtMeshTile* bestTile = 0;
        const dtPoly* bestPoly = 0;
        m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);

        // Get parent poly and tile.
        dtPolyRef parentRef = 0;
        const dtMeshTile* parentTile = 0;
        const dtPoly* parentPoly = 0;
        if (bestNode->pidx)
            parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
        if (parentRef)
            m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);

        for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
        {
            const dtLink* link = &bestTile->links[i];
            dtPolyRef neighbourRef = link->ref;
            // Skip invalid neighbours and do not follow back to parent.
            if (!neighbourRef || neighbourRef == parentRef)
                continue;

            // Expand to neighbour
            const dtMeshTile* neighbourTile = 0;
            const dtPoly* neighbourPoly = 0;
            m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);

            // Do not advance if the polygon is excluded by the filter.
            if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
                continue;

            // Find edge and calc distance to the edge.
            float va[3], vb[3];
            if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
                continue;

            // If the poly is not touching the edge to the next polygon, skip the connection it.
            float tmin, tmax;
            int segMin, segMax;
            if (!dtIntersectSegmentPoly2D(va, vb, verts, nverts, tmin, tmax, segMin, segMax))
                continue;
            if (tmin > 1.0f || tmax < 0.0f)
                continue;

            dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
            if (!neighbourNode)
            {
                status |= DT_OUT_OF_NODES;
                continue;
            }

            if (neighbourNode->flags & DT_NODE_CLOSED)
                continue;

            // Cost
            if (neighbourNode->flags == 0)
                dtVlerp(neighbourNode->pos, va, vb, 0.5f);

            const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);

            // The node is already in open list and the new result is worse, skip.
            if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
                continue;

            neighbourNode->id = neighbourRef;
            neighbourNode->flags &= ~DT_NODE_CLOSED;
            neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
            neighbourNode->total = total;

            if (neighbourNode->flags & DT_NODE_OPEN)
            {
                m_openList->modify(neighbourNode);
            }
            else
            {
                if (n < maxResult)
                {
                    if (resultRef)
                        resultRef[n] = neighbourNode->id;
                    if (resultParent)
                        resultParent[n] = m_nodePool->getNodeAtIdx(neighbourNode->pidx)->id;
                    if (resultCost)
                        resultCost[n] = neighbourNode->total;
                    ++n;
                }
                else
                {
                    status |= DT_BUFFER_TOO_SMALL;
                }
                neighbourNode->flags = DT_NODE_OPEN;
                m_openList->push(neighbourNode);
            }
        }
    }

    *resultCount = n;

    return status;
}

dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius,
                                                const dtQueryFilter* filter,
                                                dtPolyRef* resultRef, dtPolyRef* resultParent,
                                                int* resultCount, const int maxResult) const
{
    dtAssert(m_nav);
    dtAssert(m_tinyNodePool);

    *resultCount = 0;

    // Validate input
    if (!startRef || !m_nav->isValidPolyRef(startRef))
        return DT_FAILURE | DT_INVALID_PARAM;

    static const int MAX_STACK = 48;
    dtNode* stack[MAX_STACK];
    int nstack = 0;

    m_tinyNodePool->clear();

    dtNode* startNode = m_tinyNodePool->getNode(startRef);
    startNode->pidx = 0;
    startNode->id = startRef;
    startNode->flags = DT_NODE_CLOSED;
    stack[nstack++] = startNode;

    const float radiusSqr = dtSqr(radius);

    float pa[DT_VERTS_PER_POLYGON*3];
    float pb[DT_VERTS_PER_POLYGON*3];

    dtStatus status = DT_SUCCESS;

    int n = 0;
    if (n < maxResult)
    {
        resultRef[n] = startNode->id;
        if (resultParent)
            resultParent[n] = 0;
        ++n;
    }
    else
    {
        status |= DT_BUFFER_TOO_SMALL;
    }

    while (nstack)
    {
        // Pop front.
        dtNode* curNode = stack[0];
        for (int i = 0; i < nstack-1; ++i)
            stack[i] = stack[i+1];
        nstack--;

        // Get poly and tile.
        // The API input has been cheked already, skip checking internal data.
        const dtPolyRef curRef = curNode->id;
        const dtMeshTile* curTile = 0;
        const dtPoly* curPoly = 0;
        m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);

        for (unsigned int i = curPoly->firstLink; i != DT_NULL_LINK; i = curTile->links[i].next)
        {
            const dtLink* link = &curTile->links[i];
            dtPolyRef neighbourRef = link->ref;
            // Skip invalid neighbours.
            if (!neighbourRef)
                continue;

            // Skip if cannot alloca more nodes.
            dtNode* neighbourNode = m_tinyNodePool->getNode(neighbourRef);
            if (!neighbourNode)
                continue;
            // Skip visited.
            if (neighbourNode->flags & DT_NODE_CLOSED)
                continue;

            // Expand to neighbour
            const dtMeshTile* neighbourTile = 0;
            const dtPoly* neighbourPoly = 0;
            m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);

            // Skip off-mesh connections.
            if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
                continue;

            // Do not advance if the polygon is excluded by the filter.
            if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
                continue;

            // Find edge and calc distance to the edge.
            float va[3], vb[3];
            if (!getPortalPoints(curRef, curPoly, curTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
                continue;

            // If the circle is not touching the next polygon, skip it.
            float tseg;
            float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
            if (distSqr > radiusSqr)
                continue;

            // Mark node visited, this is done before the overlap test so that
            // we will not visit the poly again if the test fails.
            neighbourNode->flags |= DT_NODE_CLOSED;
            neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode);

            // Check that the polygon does not collide with existing polygons.

            // Collect vertices of the neighbour poly.
            const int npa = neighbourPoly->vertCount;
            for (int k = 0; k < npa; ++k)
                dtVcopy(&pa[k*3], &neighbourTile->verts[neighbourPoly->verts[k]*3]);

            bool overlap = false;
            for (int j = 0; j < n; ++j)
            {
                dtPolyRef pastRef = resultRef[j];

                // Connected polys do not overlap.
                bool connected = false;
                for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next)
                {
                    if (curTile->links[k].ref == pastRef)
                    {
                        connected = true;
                        break;
                    }
                }
                if (connected)
                    continue;

                // Potentially overlapping.
                const dtMeshTile* pastTile = 0;
                const dtPoly* pastPoly = 0;
                m_nav->getTileAndPolyByRefUnsafe(pastRef, &pastTile, &pastPoly);

                // Get vertices and test overlap
                const int npb = pastPoly->vertCount;
                for (int k = 0; k < npb; ++k)
                    dtVcopy(&pb[k*3], &pastTile->verts[pastPoly->verts[k]*3]);

                if (dtOverlapPolyPoly2D(pa,npa, pb,npb))
                {
                    overlap = true;
                    break;
                }
            }
            if (overlap)
                continue;

            // This poly is fine, store and advance to the poly.
            if (n < maxResult)
            {
                resultRef[n] = neighbourRef;
                if (resultParent)
                    resultParent[n] = curRef;
                ++n;
            }
            else
            {
                status |= DT_BUFFER_TOO_SMALL;
            }

            if (nstack < MAX_STACK)
            {
                stack[nstack++] = neighbourNode;
            }
        }
    }

    *resultCount = n;

    return status;
}


struct dtSegInterval
{
    short tmin, tmax;
};

static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts,
                           const short tmin, const short tmax)
{
    if (nints+1 > maxInts) return;
    // Find insertion point.
    int idx = 0;
    while (idx < nints)
    {
        if (tmax <= ints[idx].tmin)
            break;
        idx++;
    }
    // Move current results.
    if (nints-idx)
        memmove(ints+idx+1, ints+idx, sizeof(dtSegInterval)*(nints-idx));
    // Store
    ints[idx].tmin = tmin;
    ints[idx].tmax = tmax;
    nints++;
}

dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter,
                                             float* segments, int* segmentCount, const int maxSegments) const
{
    dtAssert(m_nav);

    *segmentCount = 0;

    const dtMeshTile* tile = 0;
    const dtPoly* poly = 0;
    if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
        return DT_FAILURE | DT_INVALID_PARAM;

    int n = 0;
    static const int MAX_INTERVAL = 16;
    dtSegInterval ints[MAX_INTERVAL];
    int nints;

    dtStatus status = DT_SUCCESS;

    for (int i = 0, j = (int)poly->vertCount-1; i < (int)poly->vertCount; j = i++)
    {
        // Skip non-solid edges.
        nints = 0;
        if (poly->neis[j] & DT_EXT_LINK)
        {
            // Tile border.
            for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next)
            {
                const dtLink* link = &tile->links[k];
                if (link->edge == j)
                {
                    if (link->ref != 0)
                    {
                        const dtMeshTile* neiTile = 0;
                        const dtPoly* neiPoly = 0;
                        m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
                        if (filter->passFilter(link->ref, neiTile, neiPoly))
                        {
                            insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax);
                        }
                    }
                }
            }
        }
        else if (poly->neis[j])
        {
            // Internal edge
            const unsigned int idx = (unsigned int)(poly->neis[j]-1);
            const dtPolyRef ref = m_nav->getPolyRefBase(tile) | idx;
            if (filter->passFilter(ref, tile, &tile->polys[idx]))
                continue;
        }

        // Add sentinels
        insertInterval(ints, nints, MAX_INTERVAL, -1, 0);
        insertInterval(ints, nints, MAX_INTERVAL, 255, 256);

        // Store segment.
        const float* vj = &tile->verts[poly->verts[j]*3];
        const float* vi = &tile->verts[poly->verts[i]*3];
        for (int k = 1; k < nints; ++k)
        {
            // Find the space inbetween the opening areas.
            const int imin = ints[k-1].tmax;
            const int imax = ints[k].tmin;
            if (imin == imax) continue;
            if (imin == 0 && imax == 255)
            {
                if (n < maxSegments)
                {
                    float* seg = &segments[n*6];
                    n++;
                    dtVcopy(seg+0, vj);
                    dtVcopy(seg+3, vi);
                }
                else
                {
                    status |= DT_BUFFER_TOO_SMALL;
                }
            }
            else
            {
                const float tmin = imin/255.0f;
                const float tmax = imax/255.0f;
                if (n < maxSegments)
                {
                    float* seg = &segments[n*6];
                    n++;
                    dtVlerp(seg+0, vj,vi, tmin);
                    dtVlerp(seg+3, vj,vi, tmax);
                }
                else
                {
                    status |= DT_BUFFER_TOO_SMALL;
                }
            }
        }
    }

    *segmentCount = n;

    return status;
}

dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius,
                                            const dtQueryFilter* filter,
                                            float* hitDist, float* hitPos, float* hitNormal) const
{
    dtAssert(m_nav);
    dtAssert(m_nodePool);
    dtAssert(m_openList);

    // Validate input
    if (!startRef || !m_nav->isValidPolyRef(startRef))
        return DT_FAILURE | DT_INVALID_PARAM;

    m_nodePool->clear();
    m_openList->clear();

    dtNode* startNode = m_nodePool->getNode(startRef);
    dtVcopy(startNode->pos, centerPos);
    startNode->pidx = 0;
    startNode->cost = 0;
    startNode->total = 0;
    startNode->id = startRef;
    startNode->flags = DT_NODE_OPEN;
    m_openList->push(startNode);

    float radiusSqr = dtSqr(maxRadius);

    dtStatus status = DT_SUCCESS;

    while (!m_openList->empty())
    {
        dtNode* bestNode = m_openList->pop();
        bestNode->flags &= ~DT_NODE_OPEN;
        bestNode->flags |= DT_NODE_CLOSED;

        // Get poly and tile.
        // The API input has been cheked already, skip checking internal data.
        const dtPolyRef bestRef = bestNode->id;
        const dtMeshTile* bestTile = 0;
        const dtPoly* bestPoly = 0;
        m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);

        // Get parent poly and tile.
        dtPolyRef parentRef = 0;
        const dtMeshTile* parentTile = 0;
        const dtPoly* parentPoly = 0;
        if (bestNode->pidx)
            parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
        if (parentRef)
            m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);

        // Hit test walls.
        for (int i = 0, j = (int)bestPoly->vertCount-1; i < (int)bestPoly->vertCount; j = i++)
        {
            // Skip non-solid edges.
            if (bestPoly->neis[j] & DT_EXT_LINK)
            {
                // Tile border.
                bool solid = true;
                for (unsigned int k = bestPoly->firstLink; k != DT_NULL_LINK; k = bestTile->links[k].next)
                {
                    const dtLink* link = &bestTile->links[k];
                    if (link->edge == j)
                    {
                        if (link->ref != 0)
                        {
                            const dtMeshTile* neiTile = 0;
                            const dtPoly* neiPoly = 0;
                            m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
                            if (filter->passFilter(link->ref, neiTile, neiPoly))
                                solid = false;
                        }
                        break;
                    }
                }
                if (!solid) continue;
            }
            else if (bestPoly->neis[j])
            {
                // Internal edge
                const unsigned int idx = (unsigned int)(bestPoly->neis[j]-1);
                const dtPolyRef ref = m_nav->getPolyRefBase(bestTile) | idx;
                if (filter->passFilter(ref, bestTile, &bestTile->polys[idx]))
                    continue;
            }

            // Calc distance to the edge.
            const float* vj = &bestTile->verts[bestPoly->verts[j]*3];
            const float* vi = &bestTile->verts[bestPoly->verts[i]*3];
            float tseg;
            float distSqr = dtDistancePtSegSqr2D(centerPos, vj, vi, tseg);

            // Edge is too far, skip.
            if (distSqr > radiusSqr)
                continue;

            // Hit wall, update radius.
            radiusSqr = distSqr;
            // Calculate hit pos.
            hitPos[0] = vj[0] + (vi[0] - vj[0])*tseg;
            hitPos[1] = vj[1] + (vi[1] - vj[1])*tseg;
            hitPos[2] = vj[2] + (vi[2] - vj[2])*tseg;
        }

        for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
        {
            const dtLink* link = &bestTile->links[i];
            dtPolyRef neighbourRef = link->ref;
            // Skip invalid neighbours and do not follow back to parent.
            if (!neighbourRef || neighbourRef == parentRef)
                continue;

            // Expand to neighbour.
            const dtMeshTile* neighbourTile = 0;
            const dtPoly* neighbourPoly = 0;
            m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);

            // Skip off-mesh connections.
            if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
                continue;

            // Calc distance to the edge.
            const float* va = &bestTile->verts[bestPoly->verts[link->edge]*3];
            const float* vb = &bestTile->verts[bestPoly->verts[(link->edge+1) % bestPoly->vertCount]*3];
            float tseg;
            float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);

            // If the circle is not touching the next polygon, skip it.
            if (distSqr > radiusSqr)
                continue;

            if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
                continue;

            dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
            if (!neighbourNode)
            {
                status |= DT_OUT_OF_NODES;
                continue;
            }

            if (neighbourNode->flags & DT_NODE_CLOSED)
                continue;

            // Cost
            if (neighbourNode->flags == 0)
            {
                getEdgeMidPoint(bestRef, bestPoly, bestTile,
                                neighbourRef, neighbourPoly, neighbourTile, neighbourNode->pos);
            }

            const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);

            // The node is already in open list and the new result is worse, skip.
            if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
                continue;

            neighbourNode->id = neighbourRef;
            neighbourNode->flags &= ~DT_NODE_CLOSED;
            neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
            neighbourNode->total = total;

            if (neighbourNode->flags & DT_NODE_OPEN)
            {
                m_openList->modify(neighbourNode);
            }
            else
            {
                neighbourNode->flags |= DT_NODE_OPEN;
                m_openList->push(neighbourNode);
            }
        }
    }

    // Calc hit normal.
    dtVsub(hitNormal, centerPos, hitPos);
    dtVnormalize(hitNormal);

    *hitDist = sqrtf(radiusSqr);

    return status;
}

bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const
{
    if (!m_nodePool) return false;
    const dtNode* node = m_nodePool->findNode(ref);
    return node && node->flags & DT_NODE_CLOSED;
}
